/*
 * =====================================================================================
 *
 *       Filename:  telnet_thread.c
 *
 *    Description:  This file provides a telnet thread, which is based on
 *                  `tcps` project in nut app directory
 *
 *        Version:  1.0
 *        Created:  2016-11-14
 *       Revision:  0.2
 *       Compiler:  WinARM
 *
 *         Author:  FengJie<fengjie@toecsec.com>
 *        Company:  TOEC Security
 *
 * =====================================================================================
 */

/*
 * TODO:
 *  - Add application protocol
 *  - Add MD5 verify
 *  - When reveiving failed or interrupt, reboot to old version
 */

/*
 * Usage/Test Method:
 * Enter "telnet x.x.x.x in a terminal.
 * When connection established, use `help` command
 * to get what commands can be used.
 */

#include <cfg/os.h>
#include <string.h>
#include <stdio.h>
#include <io.h>

#include <arch/arm.h>
#include <arch/arm/at91_efc.h>

#include <dev/board.h>
#include <dev/reset.h>

#include <sys/atom.h>
#include <sys/version.h>
#include <sys/heap.h>
#include <sys/thread.h>
#include <sys/timer.h>
#include <sys/socket.h>

#include <arpa/inet.h>
#include <pro/dhcp.h>

#ifdef NUTDEBUG
#include <sys/osdebug.h>
#include <net/netdebug.h>
#endif

#include <sys/confnet.h>

#include "AT91SAM7X256.h"
#include "version.h"
#include "utils.h"

#define FLASH_WRITE_WAIT__  60000
#define RECV_BIN_EE_OFFSET  0x20000 /* 128KB */

static char recv_buf[AT91C_IFLASH_PAGE_SIZE];
typedef uint32_t flashdat_t;
typedef volatile flashdat_t *flashptr_t;

/*
 * To save RAM, we store large strings in program space. With AVRGCC we
 * would be able to use the PSTR() macro and put the text directly in
 * the statement that uses it. But ICCAVR doesn't support anything like
 * this. Sigh.
 */
prog_char banner_P[] = "TOECSEC " PROJ_DESC "\r\n"
                       "OS Version: %s\r\n"
                       "Firmware Version: " FM_VER "\r\n"
                       "Type help to get help information.\r\n";
prog_char help_P[] = "m[emory]\tQueries number of RAM bytes free.\r\n"
                     "t[hreads]\tLists all created threads.\r\n"
                     "ti[mers]\tLists all running timers.\r\n"
                     "reboot\tRestart the target board.\r\n"
                     "q[uit]\t\tTerminates connection.\r\n";
prog_char thread_intro_P[] = "List of threads with name, state, prio, stack, mem, timeout follows\r\n";
prog_char timer_intro_P[] = "List of timers with ticks left and interval follows\r\n";
prog_char mem_fmt_P[] = "%u bytes RAM free\r\n";

RAMFUNC int __memcmp(CONST void *s1, CONST void *s2, size_t n)
{
    if (n) {
        CONST unsigned char *p1 = s1, *p2 = s2;

        do {
            if (*p1++ != *p2++) {
                return (*--p1 - *--p2);
            }
        } while (--n);
    }
    return 0;
}

RAMFUNC int __At91EfcCmd(unsigned int cmd, uint32_t tmo)
{
    int rc = 0;
    unsigned int fsr;

    /* Make sure that the previous command has finished. */
    while ((inr(MC_FSR) & MC_FRDY) == 0) {
        if (tmo && --tmo < 1) {
            return -1;
        }
    }

    /* Write command. */
    outr(MC_FCR, MC_KEY | cmd);

    /* Wait for ready flag set. */
    while (((fsr = inr(MC_FSR)) & MC_FRDY) == 0) {
        if (tmo && --tmo < 1) {
            rc = -1;
            break;
        }
    }

    /* Check result. */
    if (fsr & (MC_LOCKE | MC_PROGE)) {
        rc = -1;
    }
    return rc;
}

RAMFUNC int __At91EfcRegionLock(unsigned int off)
{
    return __At91EfcCmd((off & MC_PAGEN_MASK) | MC_FCMD_SLB, FLASH_WRITE_WAIT__);
}

RAMFUNC int __At91EfcRegionUnlock(unsigned int off)
{
    return __At91EfcCmd((off & MC_PAGEN_MASK) | MC_FCMD_CLB, FLASH_WRITE_WAIT__);
}

RAMFUNC int __At91EfcSectorWrite(unsigned int off, CONST void *data, unsigned int len)
{
    flashptr_t dp = (flashptr_t) (uptr_t) (AT91C_IFLASH + off);
    int rc;
    unsigned int i;

    if (data) {
        flashptr_t sp = (flashptr_t) data;

        /* Copy data to the flash write buffer. */
        for (i = 0; i < len; i += sizeof(flashdat_t)) {
            *dp++ = *sp++;
        }
    }
    else {
        /* All bits set to emulate sector erasing. */
        for (i = 0; i < len; i += sizeof(flashdat_t)) {
            *dp++ = (flashdat_t)(-1);
        }
    }

    /* Clear NEBP in mode register for automatic erasure. */
    outr(MC_FMR, (i = inr(MC_FMR)) & ~MC_NEBP);
    /* Execute page write command. */
    rc = __At91EfcCmd((off & MC_PAGEN_MASK) | MC_FCMD_WP, FLASH_WRITE_WAIT__);
    /* Restore mode register. */
    outr(MC_FMR, i);

    return rc;
}

int write_page_to_buf_zone(unsigned int pos, CONST void *data)
{
    int rc = -1;
    char page[AT91C_IFLASH_PAGE_SIZE];

    rc = At91EfcSectorRead(RECV_BIN_EE_OFFSET + pos, page, AT91C_IFLASH_PAGE_SIZE);
    /* Compare old with new contents. */
    if (memcmp(page, data, AT91C_IFLASH_PAGE_SIZE)) {
        /* New contents differs. Copy it into the sector buffer. */
        memcpy(page, data, AT91C_IFLASH_PAGE_SIZE);
        /* Write back new data. Maintain region lock. */
        if (At91EfcRegionUnlock(RECV_BIN_EE_OFFSET + pos) == 0) {
            rc = At91EfcSectorWrite(RECV_BIN_EE_OFFSET + pos, page, AT91C_IFLASH_PAGE_SIZE);
            At91EfcRegionLock(RECV_BIN_EE_OFFSET + pos);
        }
    }

    return rc;
}

RAMFUNC void copy_bin_to_main_region(unsigned int code_len)
{
    int rc = -1;
    unsigned int src_off = AT91C_IFLASH + RECV_BIN_EE_OFFSET;
    unsigned int dst_off = AT91C_IFLASH;
    unsigned int off = 0;
    char src_buf[AT91C_IFLASH_PAGE_SIZE];
    char dst_buf[AT91C_IFLASH_PAGE_SIZE];
    int i;

    /* IRQ handlers are located in flash. Disable them. */
    NutEnterCritical();

    while (code_len > AT91C_IFLASH_PAGE_SIZE) {
        for (i = 0; i < AT91C_IFLASH_PAGE_SIZE; i++) {
            dst_buf[i] = *(char *)(dst_off + i);
        }
        for (i = 0; i < AT91C_IFLASH_PAGE_SIZE; i++) {
            src_buf[i] = *(char *)(src_off + i);
        }
        /* Compare old with new contents. */
        if (__memcmp(dst_buf, src_buf, AT91C_IFLASH_PAGE_SIZE)) {
            /* Write back new data. Maintain region lock. */
            if (__At91EfcRegionUnlock(off) == 0) {
                rc = __At91EfcSectorWrite(off, src_buf, AT91C_IFLASH_PAGE_SIZE);
                __At91EfcRegionLock(off);
            } else {
                continue;
            }
        }
        src_off += AT91C_IFLASH_PAGE_SIZE;
        dst_off += AT91C_IFLASH_PAGE_SIZE;
        off += AT91C_IFLASH_PAGE_SIZE;
        code_len -= AT91C_IFLASH_PAGE_SIZE;
    }

    if (code_len > 0) {
        for (i = 0; i < AT91C_IFLASH_PAGE_SIZE; i++) {
            dst_buf[i] = *(char *)(dst_off + i);
        }
        for (i = 0; i < AT91C_IFLASH_PAGE_SIZE; i++) {
            src_buf[i] = *(char *)(src_off + i);
        }
        /* Compare old with new contents. */
        if (__memcmp(dst_buf, src_buf, code_len)) {
            /* Write back new data. Maintain region lock. */
            if (__At91EfcRegionUnlock(off) == 0) {
                rc = __At91EfcSectorWrite(off, src_buf, code_len);
                __At91EfcRegionLock(off);
            }
        }
    }

    /* soft reset */
    REG32(RSTC_CR) = 0xA500000D;
}

/*
 * Process client requests.
 */
void ProcessRequests(FILE * stream)
{
    int got;
    char *cp;
    size_t ret;
    unsigned int off;
    unsigned int bin_len;

    /*
     * Send a welcome banner.
     */
    fprintf_P(stream, banner_P, NutVersionString());
    for (;;) {

        /*
         * Flush output and read a line.
         */
        fflush(stream);
        if (fgets(recv_buf, sizeof(recv_buf), stream) == 0)
            break;

        /*
         * Chop off EOL.
         */
        if ((cp = strchr(recv_buf, '\r')) != 0)
            *cp = 0;
        if ((cp = strchr(recv_buf, '\n')) != 0)
            *cp = 0;

        /*
         * Ignore blank lines.
         */
        got = strlen(recv_buf);
        if (got == 0) {
            continue;
        }

        /*
         * Update firmware
         */
        if (strncmp(recv_buf, "update", got) == 0) {
            fprintf(stream, "ready\r\n");
            fflush(stream);
            bin_len = 0;
            off = 0;
            while (1) {
                ret = fread(recv_buf, 1, sizeof(recv_buf), stream);
                if (ret > 0) {
                    /* Write to flash */
                    write_page_to_buf_zone(off, recv_buf);
                    /* off is page align, but bin_len is the real length of bin file */
                    off += AT91C_IFLASH_PAGE_SIZE;
                    bin_len += ret;
                } else {
                    break;
                }
            }
            copy_bin_to_main_region(bin_len);
        }

        if (strncmp(recv_buf, "reboot", got) == 0) {
            NutReset();
        }

        /*
         * Memory info.
         */
        if (strncmp(recv_buf, "memory", got) == 0) {
            fprintf_P(stream, mem_fmt_P, (u_int)NutHeapAvailable());
            continue;
        }

        /*
         * List threads.
         */
        if (strncmp(recv_buf, "threads", got) == 0) {
            NUTTHREADINFO *tdp;
            NUTTIMERINFO *tnp;

            fputs_P(thread_intro_P, stream);
            for (tdp = nutThreadList; tdp; tdp = tdp->td_next) {
                fputs(tdp->td_name, stream);
                switch (tdp->td_state) {
                case TDS_TERM:
                    fputs("\tTerm\t", stream);
                    break;
                case TDS_RUNNING:
                    fputs("\tRun\t", stream);
                    break;
                case TDS_READY:
                    fputs("\tReady\t", stream);
                    break;
                case TDS_SLEEP:
                    fputs("\tSleep\t", stream);
                    break;
                }
                fprintf(stream, "%u\t%u", tdp->td_priority, (u_int) tdp->td_sp - (u_int) tdp->td_memory);
                if (*((u_long *) tdp->td_memory) != DEADBEEF)
                    fputs("\tCorrupted\t", stream);
                else
                    fputs("\tOK\t", stream);

                if ((tnp = (NUTTIMERINFO *) tdp->td_timer) != 0)
                    fprintf(stream, "%lu\r\n", tnp->tn_ticks_left);
                else
                    fputs("None\r\n", stream);
            }
            fputs(".\r\n", stream);
            continue;
        }

        /*
         * List timers.
         */
        if (strncmp("timers", recv_buf, got) == 0) {
            NUTTIMERINFO *tnp;

            fputs_P(timer_intro_P, stream);
            for (tnp = nutTimerList; tnp; tnp = tnp->tn_next) {
                fprintf(stream, "%lu\t", tnp->tn_ticks_left);
                if (tnp->tn_ticks)
                    fprintf(stream, "%lu\r\n", tnp->tn_ticks);
                else
                    fputs("Oneshot\r\n", stream);
            }
            fputs(".\r\n", stream);
            continue;
        }

        /*
         * Quit connection.
         */
        if (strncmp("quit", recv_buf, got) == 0) {
            break;
        }

        /*
         * Display help text on any unknown command.
         */
        fputs_P(help_P, stream);
    }
}

/*
 * Thread handler
 */
THREAD(telnet, arg)
{
    TCPSOCKET *sock;
    FILE *stream;

    /*
     * Now loop endless for connections.
     */
    for (;;) {
        /*
         * Create a socket.
         */
        if ((sock = NutTcpCreateSocket()) != 0) {
            /*
             * Listen on port 23. If we return, we got a client.
             */
            printf("Waiting for a telnet client...");
            if (NutTcpAccept(sock, 23) == 0) {
                puts("connected");

                /*
                 * Open a stream and associate it with the socket, so
                 * we can use standard I/O. Note, that socket streams
                 * currently do support text mode.
                 */
                if ((stream = _fdopen((int) sock, "r+b")) != 0) {
                    /*
                     * Process client requests.
                     */
                    ProcessRequests(stream);
                    puts("Disconnected");

                    /*
                     * Close the stream.
                     */
                    fclose(stream);
                } else
                    puts("Assigning a stream failed");
            } else
                puts("failed");

            /*
             * Close our socket.
             */
            NutTcpCloseSocket(sock);
        }
    }
}

void telnet_thread_create(void)
{
    NutThreadCreate("Telnet", telnet, 0, 1024);
}
